#include "ANN_distance_field.h"

namespace Field_packing
{
	ANN_distance_field_2::ANN_distance_field_2(const std::vector< Packing_polygon* >& polygon_set, unsigned int sample_rate)
		: nb_polygons(polygon_set.size())
	{
		kdtrees.reserve(nb_polygons);
		local_frames.reserve(nb_polygons);
		// construct local coordinate systems
		for (size_t i = 0; i < nb_polygons; i++)
		{
			Point_2 c = polygon_set[i]->centroid();
			local_frames.push_back(Local_frame_2(Vector_2(1, 0), Vector_2(0, 1), c));
		}
		// sample the edges of polygons and construct the kdtrees
		std::valarray<double> perimeters(nb_polygons);
		std::transform(polygon_set.begin(), polygon_set.end(), &perimeters[0], std::mem_fun(&Packing_polygon::perimeter));
		double max_peri = perimeters.max();
		for (size_t i = 0; i < nb_polygons; i++)
		{
			// sampling
			unsigned int nb_samp_pnts = perimeters[i]/max_peri * sample_rate;
			nb_samp_pnts = std::max<unsigned int>(nb_samp_pnts, 1);
			Polygon_sampler ps(*polygon_set[i]);
			std::vector<Point_2> sampl_pnts;
			ps.edge_sample(nb_samp_pnts, sampl_pnts);
			// kd-tree construction
			//Shared_ptr sp(annAllocPts(sampl_pnts.size(), 2), annDeallocPts);
			ANNpointArray pa = annAllocPts(sampl_pnts.size(), 2);
			for (size_t j = 0; j < sampl_pnts.size(); j++)
			{
				// the kd-tree is built in the local coordinate of the polygon
				Point_2 local_pnt = local_frames[i].local_coord(sampl_pnts[j]);
				pa[j][0] = local_pnt.x();
				pa[j][1] = local_pnt.y();
			}
			point_pool.push_back(pa);
			kdtrees.push_back(boost::shared_ptr<ANNkd_tree>(new ANNkd_tree(pa, sampl_pnts.size(), 2)));
		}
	}
	
	ANN_distance_field_2::~ANN_distance_field_2()
	{
		for_each(point_pool.begin(), point_pool.end(), annDeallocPts);
		annClose();
	}

	double ANN_distance_field_2::nearest_point_at(const Point_2& p, size_t polygon_id, Point_2& nearest_pnt) const
	{
		ANNpoint query_pnt = annAllocPt(2);
		Point_2 local_coord = local_frames[polygon_id].local_coord(p);
		query_pnt[0] = local_coord.x();
		query_pnt[1] = local_coord.y();
		int idx;
		double dist;
		kdtrees[polygon_id]->annkSearch(query_pnt, 1, &idx, &dist, 0);
		double *pnts = point_pool[polygon_id][idx];
		// result in the local coordinate, so go back to the world coordinate
		nearest_pnt = local_frames[polygon_id].world_coord(Point_2(pnts[0], pnts[1]));
		annDeallocPt(query_pnt);
		return dist;
	}

	//void ANN_distance_field_2::update_distance_field(size_t polygon_id, const Transformation_2& transformation)
	//{
	//}
}